
import lejos.nxt.LCD;
import lejos.nxt.comm.RConsole;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
import lejos.util.Stopwatch;

import java.lang.Math;
public class FollowWall extends RobotMoving implements Behavior {

	private static final int OUT_OF_RANGE = 255;

	private int[] distances = {0,0,0,0};
	private int startIndex = 0;
	
	private int lastAverage = 0;
	private int newAverage = 0;

	private boolean shouldStop = false;
	private boolean rotating = false;

	private int totalAngle = 0;

	private int foundWall = 0;
	public FollowWall() {
		//RConsole.open();
		//RConsole.openBluetooth(0);
		RConsole.println("Created Follow Wall Behavior");		
	}

	@Override
	public void action() {
		
		if(rotating) {
			return;
		}

		// Aids Suppress
		if(shouldStop) {
			return;
		}

		//First Distance
		Delay.msDelay(20);
		int d = wallSensor.getDistance();
		pilot.travel(10, true);
		// Validate 1
		if(d == 0 || d >= 255) {
			return;
		}
		
		// Store D
		distances[startIndex] = d;

		// Check enough readings
		if(startIndex < 3) {
			startIndex++;
			return;
		}

		// Aids suppress
		if(shouldStop) {
			return;
		}
		
		RConsole.println("D1: " + distances[startIndex - 1] + "  D2: " + distances[startIndex - 2] +
		"  D3: " + distances[startIndex - 3]);
		
		// Take median of last three readings
		if(distances[startIndex - 1] >=  distances[startIndex - 2]) {
			
			if(distances[startIndex - 1] >=  distances[startIndex - 3]) {
				//Found Max
				newAverage = Math.max(distances[startIndex - 2], distances[startIndex - 3]);
			} else {
				newAverage = distances[startIndex - 1];
			}
		} else {
			if(distances[startIndex - 1] >=  distances[startIndex - 3]) {
				newAverage = distances[startIndex - 1];
			} else {
				newAverage = Math.max(distances[startIndex - 2], distances[startIndex - 3]);
			}	
		}

		startIndex++;

		// Loop index
		if(startIndex >= 4) {
			startIndex = 0;
		}

		// First time taken an average
		if(lastAverage == 0) {
			lastAverage = newAverage;
			return;
		}

		// Aids Suppress
		if(shouldStop) {
			return;
		}

		if(newAverage - lastAverage >= 10) {
			totalAngle=20;
		} else if(newAverage - lastAverage >= 4) {
			totalAngle=15;
		} else if(newAverage - lastAverage >= 2) {
			totalAngle=10;
		} else if(newAverage - lastAverage == 1) {
			totalAngle = 8;
		} else if(newAverage - lastAverage <= -10) {
			totalAngle =-20;
		} else if(newAverage - lastAverage <= -5) {
			totalAngle =-15;
		} else if(newAverage - lastAverage <= -2) {
			totalAngle =-10;
		} else if(newAverage - lastAverage == -1) {
			totalAngle = -8;
		} else {
			return;
		}
		
		// Aids Suppress
		if(shouldStop) {
			return;
		}
		
RConsole.println("Last Average: " + lastAverage);
RConsole.println("New Average: " + newAverage);
RConsole.println("Angle: " + totalAngle);


		//pilot.steer(90, turnAngle, true);
		pilot.rotate(totalAngle, true);
		rotating = true;
		try {
			Thread.sleep(300);
		} catch(Exception e) {
			System.out.println("Problem Waiting! Just Carrying on");
		}
		
		for(int i=0; i<4; i++) {
			distances[i] = 0;
		}
		startIndex = 0;
		lastAverage = 0;
		
		//pilot.forward();
		pilot.travel(10);
		rotating = false;
	}

	@Override
	public void suppress() {
		RConsole.println("Suppress Follow Wall.");
		shouldStop = true;
		pilot.stop();
	}


	@Override
	public boolean takeControl() {
		// We want to take control if we are now facing close enough to a wall
		
		Delay.msDelay(20);
		int distance = wallSensor.getDistance();
		
		if(distance <= 50) {
			shouldStop = false;
			foundWall++;
			if(foundWall > 2) {
				setByWall(true);
				foundWall = 0;
			}
			
			return true;
		}
		
		return false;
	}
}